#ifndef PCLMATCH_H
#define PCLMATCH_H

#include "Params.h"
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <Eigen/Dense>
#include <pcl/features/normal_3d.h>
#include <pcl/keypoints/harris_3d.h>
#include <pcl/keypoints/sift_keypoint.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#include <cmath>

class PclMatch
{
public:
    PclMatch(std::shared_ptr<Params>& params_);
    // 通用的发布点云函数
    void publishPointCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, ros::Publisher& pub);

    // 将二维点云扩展为三维点云（z坐标设置为0），并添加intensity字段
    pcl::PointCloud<pcl::PointXYZI>::Ptr expandPointCloudTo3D(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud);

    // 检查两个点云间距离大于0.01米，如果大于0.01米，且小于0.1米则连续插入点云，直到点云间距小于0.01米
    pcl::PointCloud<pcl::PointXYZI>::Ptr checkAndInsertPoints(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, float min_distance, float max_distance);

    // 对点云数据进行预处理，包括下采样和去除噪声。
    pcl::PointCloud<pcl::PointXYZI>::Ptr preprocessPointCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_msg, int mean_k = 50, double stddev_mul_thresh = 1.0);

    // 根据输入的点云拟合线段，并发布所有线段的点云。
    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> fitLines(const pcl::PointCloud<pcl::PointXYZI>::Ptr& points, ros::Publisher& line_pub, int size_threshold = 100);

    // 检查线段组中是否存在直角
    Eigen::Vector3f checkRightAngleInLines(const std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>& lines, ros::Publisher& angle_pub, size_t step = 10);

    // 回调函数，用于处理接收到的点云数据
    void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg);

private:
    ros::Subscriber sub_point_cloud;
    
    ros::Publisher pub_point_cloud;
    ros::Publisher pub_voxel;
    ros::Publisher pub_cross;
    ros::Publisher pub_keypoints;
    ros::Publisher pub_interpolated;
    ros::Publisher pub_curve;
        // 定义params
    std::shared_ptr<Params> params;
};

#endif // PCLMATCH_H